
import os

import ament_index_python.packages

from launch import LaunchDescription
from launch_ros.actions import ComposableNodeContainer
from launch_ros.descriptions import ComposableNode

import yaml


def generate_launch_description():
    share_dir = ament_index_python.packages.get_package_share_directory('to_pointcloud')
    params_file = os.path.join(share_dir, 'config', 'qs176_cloud_params.yaml')
    with open(params_file, 'r') as f:
        params = yaml.safe_load(f)['to_pointcloud_node']['ros__parameters']
    params['calibration'] = os.path.join(share_dir, 'params', 'CZLidar_QS176_ZW1_34.yaml')
    container = ComposableNodeContainer(
            name='lidar209_pointcloud_container',
            namespace='',
            package='rclcpp_components',
            executable='component_container',
            composable_node_descriptions=[
                ComposableNode(
                    package='to_pointcloud',
                    plugin='lidar209_pointcloud::Convert',
                    name='to_pointcloud_node',
                    parameters=[params]),
            ],
            output='both',
    )

    return LaunchDescription([container])
